package simulator.robot;

import simulator.labyrinth.Labyrinth;
import simulator.labyrinth.Square;
import simulator.labyrinth.Wall;
import simulator.position.Point;
import simulator.position.Position;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Move.MoveType;
import lejos.robotics.navigation.MoveListener;
import lejos.robotics.navigation.MoveProvider;
import lejos.robotics.navigation.RotateMoveController;

public class Simulator implements RotateMoveController, Runnable {
	
	private Move moveToDo;
	private Move moveDone;
	private static final double MAX_TRAVEL_SPEED = 410.0;
	private static final double MAX_ROTATE_SPEED = 301.0;
	private static final int UPDATES_PER_SECOND = 50;
	private boolean leftIsPressed;
	private boolean rightIsPressed;
	
	private static boolean RUIS_RIJDEN = false;
	
	private Labyrinth labyrinth;
	
	private MoveListener moveListener = new MoveListener() {
		
		@Override
		public void moveStopped(Move event, MoveProvider mp) {
			
		}
		
		@Override
		public void moveStarted(Move event, MoveProvider mp) {
			
		}
	};
	private Thread thread;
	private Position position;
	private SensorMotor sensorMotor = new SensorMotor();
	private final Object lock = new Object();
	
	public Simulator() {
		reset();
		thread = new Thread(this);
		thread.start();
	}
	
	public void reset() {
		sensorMotor.resetTachoCount();
		leftIsPressed = false;
		rightIsPressed = false;
		moveToDo = new Move(MoveType.STOP, 0, 0, 15F, 100F, false);
		moveDone = new Move(MoveType.STOP, 0, 0, 15F, 100F, false);
		setPosition(20, 20, 0);
	}
	
	public void setRuis(boolean ruis) {
		RUIS_RIJDEN = ruis;
	}

	@Override
	public void run() {
		while (true) {
			synchronized (lock) {
				try {
					// We kunnen dit doen, omdat het thread het lock tijdelijk
					// zal releasen tijdens het wachten
					lock.wait();
				} catch (InterruptedException e1) {
					e1.printStackTrace();
				}
				switch (moveToDo.getMoveType()) {
					case TRAVEL:
					case ROTATE:
						executeMovement(moveToDo);
					case ARC:
						// We don't support ARC.
					case STOP:
					default:
						break;
				}
			}
		}
	}
	
	public Labyrinth getLabyrinth()
	{
		return labyrinth;
	}
	
	public void setLabyrinth(Labyrinth lab)
	{
		this.labyrinth = lab;
	}

	@Override
	public Move getMovement() {
		return moveDone;
	}
	
	private Move getMoveToDo() {
		return moveToDo;
	}

	@Override
	public void addMoveListener(MoveListener listener) {
		this.moveListener = listener;		
	}

	@Override
	public void forward() {
		if (getMovement().isMoving())
			stop();
		travel(Float.POSITIVE_INFINITY, true);
	}

	@Override
	public void backward() {
		if (getMovement().isMoving())
			stop();
		travel(Float.NEGATIVE_INFINITY, true);
	}

	@Override
	public void stop() {
		// setValues negeert het meegegeven type! Dit wordt berekend a.d.h.v.
		// de distance- en angle-parameters. Deze moeten dus op 0 staan om
		// het MoveType op STOP te krijgen.
		getMoveToDo().setValues(MoveType.STOP, 0, 0, false);
		
		// moveToDo op STOP zetten zal ervoor zorgen dat het andere thread stopt met uitvoeren.
		// Dat thread is gesynchronizeerd op lock. Wanneer die klaar is, kunnen we ook de moveDone aanpassen
		synchronized (lock) {
			getMovement().setValues(MoveType.STOP, 0, 0, false);
		}
	}

	@Override
	public boolean isMoving() {
		return getMoveToDo().isMoving();
	}

	@Override
	public void travel(double distance) {
		travel(distance, false);
	}

	@Override
	public void travel(double distance, boolean immediateReturn) {
		Move move = new Move(MoveType.TRAVEL, (float) distance, 0, (float) getTravelSpeed(), (float) getRotateSpeed(), true);
		executeMovement(move, immediateReturn);
	}
	
	/**
	 * Laat de simulator een Move uitvoeren. Supported zijn: TRAVEL en ROTATE
	 * 
	 * @param	move
	 * 			De uit te voeren move
	 * @param	immediateReturn
	 * 			Bepaalt of de move op een aparte thread uitgevoerd moet worden (true)
	 * 			of de oproepende thread moet locken tot hij klaar is (false)
	 */
	public void executeMovement(Move move, boolean immediateReturn) {
		// Stop de huidige beweging
		if (getMovement().isMoving())
			stop();
		
		moveToDo = move;
		if (!immediateReturn)
			executeMovement(move);
		else
			synchronized (lock) {
				// Dit moet in een synchronized block staan. Mocht hier ooit
				// nog iets toegevoegd worden, note to self: na lock.notify()
				// moet het lock zo snel mogelijk released worden.
				lock.notify();
			}
	}
	
	/**
	 * Voer een move uit met de robot. Supported zijn: TRAVEL en ROTATE
	 * 
	 * @param   move
	 * 			Deze parameter is nog nodig (moveToDo is niet genoeg) in gevallen wanneer
	 * 			de beweging onderbroken wordt. Als hij in het midden van de uitvoering zat,
	 * 			zou hij anders mogelijk nog één stap fout doen vooraleer te stoppen
	 */
	private void executeMovement(Move move) {
		moveListener.moveStarted(move, this);
		
		float maxMovement = Math.abs(getMoveDistance(move));
		
		getMovement().setValues(move.getMoveType(), 0, 0, true);
		
		float movementDone = 0;
		while (isMoving() && Math.abs(movementDone) < maxMovement) {
			double dist = Math.signum(getMoveDistance(move)) * getSpeedOfMovement(move) / UPDATES_PER_SECOND;

			// De beweging gebeurt in stappen ipv continu. Wanneer we hierdoor te ver
			// zouden belanden, moeten we zorgen dat we op de maximale afstand komen
			// te staan
			if (Math.abs(movementDone + dist) > maxMovement)
				dist = getMoveDistance(move) - movementDone;
			
			// Stop wanneer we tegen een muur zitten.
			if (collisionDetect(getPosition(), move.getMoveType(), dist))
				break;
			
			movementDone += dist;
			getMovement().setValues(move.getMoveType(),
					(move.getMoveType() == MoveType.TRAVEL ? movementDone : 0),
					(move.getMoveType() == MoveType.ROTATE ? movementDone : 0),
					true);
			
			applyMovement(getPosition(), move.getMoveType(), dist);
			
			try {
				Thread.sleep(1000 / UPDATES_PER_SECOND);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		
		if (RUIS_RIJDEN) {
			double dist = 2 * Math.random() - 1; // Random value tussen -1 en 1 
			movementDone += dist;
			getMovement().setValues(move.getMoveType(),
					(move.getMoveType() == MoveType.TRAVEL ? movementDone : 0),
					(move.getMoveType() == MoveType.ROTATE ? movementDone : 0),
					true);
			applyMovement(getPosition(), move.getMoveType(), dist);
		}
		
		stop();
		moveListener.moveStopped(move, this);
	}

	/**
	 * Selecteert aan de hand van de MoveType de juiste speed
	 */
	private float getSpeedOfMovement(Move move) {
		switch (move.getMoveType()) {
			case TRAVEL:
				return move.getTravelSpeed();
			case ROTATE:
				return move.getRotateSpeed();
			case ARC:
			default:
				return 0;
		}
	}

	/**
	 * Selecteert aan de hand van de MoveType de juiste distance/angle
	 */
	private float getMoveDistance(Move move) {
		switch (move.getMoveType()) {
			case TRAVEL:
				return move.getDistanceTraveled();
			case ROTATE:
				return move.getAngleTurned();
			case ARC:
			default:
				return 0;
		}
		
	}

	/**
	 * Controleert of de robot na het bewegen "door" een muur zou zitten
	 * 
	 * @param   thisPosition
	 * 			De huidige positie van de robot
	 * @param   type
	 * 			Het type van de beweging (TRAVEL of ROTATE)
	 * @param   dist
	 * 			De grootte van de beweging
	 * @return
	 * 			Of de robot tegen een muur aan zit of niet
	 */
	private boolean collisionDetect(Position thisPosition, MoveType type, double dist) {
		// If we're not driving in a labyrinth, this function doesn't have to do anything
		if (labyrinth == null)
			return false;
		
		Position nextPosition = thisPosition.clone();
		
		applyMovement(nextPosition, type, dist);
		
		// [0] is top left, [1] is top right
		Point[] corners = getCornersOfRobot(nextPosition);
		
		// TODO: Moeten de touchsensoren ook aan gaan bij een collision in het midden?
		updateTouchSensors(corners[0], corners[1]);
		
		for (int i = 0; i < corners.length; ++i) {
			if (hitsWall(corners[i]))
				return true;
		}
		
		return false;
	}
	
	private void updateTouchSensors(Point sensor1, Point sensor2) {
		leftIsPressed = hitsWall(sensor1);
		rightIsPressed = hitsWall(sensor2);
	}

	private boolean hitsWall(Point point) {
		Square square = labyrinth.getSquareFromPoint(point);
		for (Wall w : square.getWalls()) {
			if (w.findIntersection(point))
				return true;
		}
		
		return false;
	}

	/**
	 * Pas een beweging toe op een Position
	 */
	private void applyMovement(Position thisPosition, MoveType type, double dist) {
		switch (type) {
			case TRAVEL:
				thisPosition.move(dist);
				break;
			case ROTATE:
				thisPosition.turn(dist);
			case ARC:
			default:
				return;
		}
	}

	@Override
	public void setTravelSpeed(double speed) {
	    if (speed > getMaxTravelSpeed())
	        speed = getMaxTravelSpeed();
		if (speed != getMoveToDo().getTravelSpeed())
			moveToDo = new Move(getMoveToDo().getMoveType(), getMoveToDo().getDistanceTraveled(), getMoveToDo().getAngleTurned(), (float) speed, getMoveToDo().getRotateSpeed(), getMoveToDo().isMoving());
	}

	@Override
	public double getTravelSpeed() {
		return (double) getMoveToDo().getTravelSpeed();
	}

	@Override
	public double getMaxTravelSpeed() {
		return MAX_TRAVEL_SPEED;
	}

	@Override
	public void rotate(double angle) {
		rotate(angle, false);
	}

	@Override
	public void rotate(double angle, boolean immediateReturn) {
		Move move = new Move(MoveType.ROTATE, 0, (float) angle, (float) getTravelSpeed(), (float) getRotateSpeed(), true);
		executeMovement(move, immediateReturn);
	}

	@Override
	public void setRotateSpeed(double speed) {
	    if (speed > getRotateMaxSpeed())
	        speed = getRotateMaxSpeed();
		if (speed != getMoveToDo().getRotateSpeed())
			moveToDo = new Move(getMoveToDo().getMoveType(),getMoveToDo().getDistanceTraveled(),getMoveToDo().getAngleTurned(),(float) getTravelSpeed(),(float) speed,getMoveToDo().isMoving());
	}

	@Override
	public double getRotateMaxSpeed() {
		return MAX_ROTATE_SPEED;
	}

	@Override
	public double getRotateSpeed() {
		return getMoveToDo().getRotateSpeed();
	}
	
	public Position getPosition() {
		return position;
	}

	public RegulatedMotor getSensorMotor() {
		return sensorMotor;
	}
	
	/**
	 * Geeft de positie van de sensor als een Point
	 */
	public Point getPositionOfSensor() {
		Position positionOfSensor = getPosition().clone();
		positionOfSensor.move(getWheelToSensorDistance());
		return positionOfSensor.getPoint();
	}
	
	public int getWheelToSensorDistance() {
		return 4;
	}
	
	/**
	 * Linker touch sensor
	 */
	public boolean leftIsPressed() {
		return leftIsPressed;
	}
	
	/**
	 * Rechter touch sensor
	 */
	public boolean rightIsPressed() {
		return rightIsPressed;
	}
	
	/**
	 * Overschrijft de positie van de robot. Handig in de GUI om fouten
	 * van de robot handmatig te corrigeren.
	 */
	public void setPosition(double x, double y, double direction) {
		position = new Position(x, y, direction);
	}
	
	/**
	 * Berekent aan de hand van de positie van het centrum van de robot
	 * de positie van de vier hoeken.
	 */
	private Point[] getCornersOfRobot(Position position) {
		Point[] positions = new Point[4];
		
		double lengthFront = 9;
		double lengthBack = 9.5F;
		double lengthSide = 8;
		
		Position pos = position.clone();
		
		// Linksvooraan
		pos.move(lengthFront);
		pos.turn(-90);
		pos.move(lengthSide);
		positions[0] = pos.getPoint();
		
		// Rechtsvooraan
		pos.move(- (lengthSide * 2));
		positions[1] = pos.getPoint();
		
		// Rechtsachteraan
		pos.turn(-90);
		pos.move(lengthFront + lengthBack);
		positions[2] = pos.getPoint();
		
		// Linksachteraan
		pos.turn(90);
		pos.move(lengthSide * 2);
		positions[3] = pos.getPoint();
		
		return positions;
	}
}
